//
// Created by lqk on 2022/2/13.
//

#include "PyBulletMotorsDriver.h"
#include <chrono>
#include <thread>
#include <ctime>
#include <sys/time.h>

//global variable
MotorsInfo g_motorsInfo{};
MotorsCommand g_motorsCommand{};
int number=0;



void PyBulletMotorsDriver::Init() {
    if(!m_lcmSub.good()) std::cout << "LCM subscriber not good!" << std::endl;
    if(!m_lcmPub.good()) std::cout << "LCM publisher not good!" << std::endl;
    m_lcmSub.subscribe(m_subChannel.c_str(),&PyBulletMotorsDriver::Callback,this);
}

void PyBulletMotorsDriver::RunOnce() {
//    std::cout<<"runonce"<<std::endl;
    ActuatorSimulate();
    SendCommand();
    TimeOutPrinter(m_lcmSub.handleTimeout(m_timeOut));

}

void PyBulletMotorsDriver::CleanUp() {

}

void PyBulletMotorsDriver::ActuatorSimulate() {
    double torqueAbad[4],torqueHip[4],torqueKnee[4];
    // * Test
//    for(int i=0;i<4;i++){
//        g_motorsCommand.kpAbad[i]=5;
//        g_motorsCommand.kpHip[i]=5;
//        g_motorsCommand.kpKnee[i]=5;
//        g_motorsCommand.kdAbad[i]=0;
//        g_motorsCommand.kdHip[i]=0;
//        g_motorsCommand.kdKnee[i]=0;
//    }
    for(int i=0;i<4;i++){
        torqueAbad[i]=g_motorsCommand.kpAbad[i]*(g_motorsCommand.qDesAbad[i] - g_motorsInfo.posAbad[i]) + g_motorsCommand.kdAbad[i] * (g_motorsCommand.qdDesAbad[i] - g_motorsInfo.velAbad[i]) + g_motorsCommand.tauAbadFF[i];
        torqueHip[i]=g_motorsCommand.kpHip[i]*(g_motorsCommand.qDesHip[i] - g_motorsInfo.posHip[i]) + g_motorsCommand.kdHip[i] * (g_motorsCommand.qdDesHip[i] - g_motorsInfo.velHip[i]) + g_motorsCommand.tauHipFF[i];
        torqueKnee[i]=g_motorsCommand.kpKnee[i]*(g_motorsCommand.qDesKnee[i] - g_motorsInfo.posKnee[i]) + g_motorsCommand.kdKnee[i] * (g_motorsCommand.qdDesKnee[i] - g_motorsInfo.velKnee[i]) + g_motorsCommand.tauKneeFF[i];
        //TODO Set torque limits

        motorsTorque[3*i]=torqueAbad[i];
        motorsTorque[3*i+1]=torqueHip[i];
        motorsTorque[3*i+2]=torqueKnee[i];
    }
//    for(auto t: motorsTorque)
//        std::cout<<t<<" ";
//    std::cout<<std::endl;
}

void PyBulletMotorsDriver::SendCommand() {
    quad_bot::torques_pb torquesPb;
//    std::cout<<"publish once"<<std::endl;
//    std::cout<<"Torques:";
    for(int i=0;i<12;i++){
        torquesPb.torques[i]=motorsTorque[i];
//        std::cout<<torquesPb.torques[i]<<" ";
    }
//    std::cout<<std::endl;
    m_lcmPub.publish(m_pubChannel.c_str(), &torquesPb);

    // * test
//    std::this_thread::sleep_for(std::chrono::milliseconds(500));
//    static int m;
//    std::cout<<m<<std::endl;
//    m++;
}

void PyBulletMotorsDriver::Callback(const lcm::ReceiveBuffer *rbuf, const std::string &channel,
                                    const quad_bot::motors_info_pb *msg) {
//    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<std::endl;
    int j=0;
    for(int i=0;i<4;i++){
        g_motorsInfo.posAbad[i]=msg->pos[j];
        g_motorsInfo.velAbad[i]=msg->vel[j];
        j++;
        g_motorsInfo.posHip[i]=msg->pos[j];
        g_motorsInfo.velHip[i]=msg->vel[j];
        j++;
        g_motorsInfo.posKnee[i]=msg->pos[j];
        g_motorsInfo.velKnee[i]=msg->vel[j];
        j++;
    }
//    std::cout<<"Pos:";
//    for(int i=0;i<12;i++){
//        std::cout<<msg->pos[i]<<" ";
//    }
//    std::cout<<std::endl;
//    std::cout<<"Vel:";
//    for(int i=0;i<12;i++){
//        std::cout<<msg->vel[i]<<" ";
//    }
//    std::cout<<std::endl;
//    std::cout<<g_motorsInfo.posAbad[0]<<std::endl;

//    std::cout<<"[LCM subscriber] callback"<<std::endl;
//    for(auto p : msg->pos)
//        std::cout<<p<<" ";
//    for(auto v : msg->vel)
//        std::cout<<v<<" ";
//    std::cout<<std::endl;
}

void PyBulletMotorsDriver::TimeOutPrinter(int lcm_handle) {
    if(!m_timeOutFlag)
        m_timeOutCounter=0;
    if(lcm_handle>0){
        m_timeOutFlag=0;
    }
    else if(lcm_handle==0){
        m_timeOutFlag=1;
        m_timeOutCounter++;
        std::cout<<"[Warning] PyBullet Motors driver connection timed out for "<<m_timeOutCounter*m_timeOut*m_period<<" seconds!"<<std::endl;
    }
}

void PyBulletMotorsDriver::PrintStatus() {
//    std::cout<<m_name<<" is running!"<<std::endl;
}